Template with STL algorithms slows down function a lot